/**
  ******************************************************************************
  * @file           : full_path_coverage.cpp
  * @author         : wangyingjie
  * @brief          : None
  * @attention      : None
  * @date           : 2025/6/3
  ******************************************************************************
  */

#include "geography/studio_proj.h"
#include "geometry/studio_geo_utils.h"
#include "geometry/studio_geo_algo.h"
#include "geography/studio_proj.h"
#include "gdal_priv.h"
#include "ogrsf_frmts.h"
#include "geometry/studio_vector_raster.h"
#include "file/studio_dir.h"
#include "motion/ccpp/studio_wave_cover_ccpp.h"
#include "motion/ccpp/studio_plow_cover_ccpp.h"

/////////// 扫描线 算法  ///////////

// 创建一个网格矢量
OGRPolygon* createGridCell(double left, double bottom, double cell_size)
{
    OGRPolygon* polygon = new OGRPolygon();
    OGRLinearRing* ring = new OGRLinearRing();
    ring->addPoint(left, bottom);
    ring->addPoint(left + cell_size, bottom);
    ring->addPoint(left + cell_size, bottom + cell_size);
    ring->addPoint(left, bottom + cell_size);
    ring->addPoint(left, bottom);

    polygon->addRing(ring);
    return polygon;
}

// 生成网格文件 geojson 文件
void generateGridAndSave(const char* filename, const double src_top, const double src_bottom, const double src_left, const double src_right, const double cell_size)
{
    GDALDriver* driver = GetGDALDriverManager()->GetDriverByName("GeoJSON");
    if (driver == nullptr)
    {
        std::cerr << "GeoJSON driver not available." << std::endl;
        return;
    }
    GDALDataset* dataset = driver->Create(filename, 0, 0, 0, GDT_Unknown, nullptr);
    if (dataset == nullptr)
    {
        std::cerr << "Failed to create output file." << std::endl;
        return;
    }
    OGRSpatialReference srs;
    srs.SetWellKnownGeogCS("WGS84");
    OGRLayer* layer = dataset->CreateLayer("grid", &srs, wkbPolygon, nullptr);
    if (layer == nullptr)
    {
        std::cerr << "Failed to create layer." << std::endl;
        return;
    }
    OGRFieldDefn fieldDefn("ID", OFTInteger);
    layer->CreateField(&fieldDefn);
    int id = 0;

    double left = std::floor(src_left / cell_size) * cell_size;
    double right = std::ceil(src_right / cell_size) * cell_size;
    double top = std::floor(src_top / cell_size) * cell_size;
    double bottom = std::ceil(src_bottom / cell_size) * cell_size;

    for (double y = bottom; y < top; y += cell_size)
    {
        for (double x = left; x < right; x += cell_size)
        {
            OGRPolygon* gridCell = createGridCell(x, y, cell_size);
            OGRFeature* feature = OGRFeature::CreateFeature(layer->GetLayerDefn());
            feature->SetField("ID", id++);
            feature->SetGeometry(gridCell);
            layer->CreateFeature(feature);
            OGRFeature::DestroyFeature(feature);
            delete gridCell;
        }
    }
    GDALClose(dataset);
}


// 往复式路径全覆盖算法
int main()
{
    std::cout << "\r\n\r\n****************" << "full_paht_coverage" << "****************" << std::endl;
    std::string root_out;
    std::string fence_root;
#ifdef IS_WINDOWS
        root_out = "D:/9_data/2_readfile/geojson/full_cover/";
        fence_root = "D:/9_data/2_readfile/geojson/fence/";
#else
    root_out = "/home/wyj/myself/2_data/2_geojson/full_cover/";
    fence_root = "/home/wyj/myself/2_data/2_geojson/fence/";
#endif

    studio_geo_utils::init_gdal_env();

    std::vector<std::string> all_fences = studio_dir::getFilesOrDirs(fence_root);
    for (auto& fence : all_fences)
    {
        std::string fileName = std::filesystem::path(fence).filename().stem().string();
        std::string out_grade = std::filesystem::path(root_out).append(fileName + "_grade.geojson").string();
        std::string out_grade_full = std::filesystem::path(root_out).append(fileName + "_grade_full.geojson").string();
        std::string out_mid_points = std::filesystem::path(root_out).append(fileName + "_mid_points.geojson").string();
        std::string out_path = std::filesystem::path(root_out).append(fileName + "_path.geojson").string();

        double cell_m =10.0;
        std::vector<studio_geo_coll> src_colls;
        studio_poly gs_src_poly;
        if (!studio_geo_utils::read_geo_coll(fence, src_colls))
        {
            std::cout << "read geojson failed" << std::endl;
            continue;
        }

        for (auto& coll : src_colls)
        {
            if (coll.m_type == enum_geometry_type::egtPolygon)
            {
                if (coll.m_poly.outer_ring.points.empty())
                {
                    break;
                }
                studio_plow_cover_ccpp plow_cover(coll.m_poly, cell_m);
                studio_line temp_path = plow_cover.generate_path();
                studio_geo_utils::write_geo(out_path, temp_path, enum_geometry_type::egtLineString);
            }
            break;
        }
        // break;
    }

    geo_utils::destroy_gdal_env();
    return 0;
}





// // 往返式路径全覆盖算法
// int main()
// {
//     std::cout << "\r\n\r\n****************" << "full_paht_coverage" << "****************" << std::endl;
//     std::string root_out;
//     std::string fence_root;
// #ifdef IS_WINDOWS
//         root_out = "D:/9_data/2_readfile/geojson/full_cover/";
//         fence_root = "D:/9_data/2_readfile/geojson/fence/";
// #else
//     root_out = "/home/wyj/myself/2_data/2_geojson/full_cover/";
//     fence_root = "/home/wyj/myself/2_data/2_geojson/fence/";
// #endif
//
//     studio_geo_utils::init_gdal_env();
//
//     std::vector<std::string> all_fences = studio_dir::getFilesOrDirs(fence_root);
//     for (auto& fence : all_fences)
//     {
//         std::string fileName = std::filesystem::path(fence).filename().stem().string();
//         std::string out_grade = std::filesystem::path(root_out).append(fileName + "_grade.geojson").string();
//         std::string out_grade_full = std::filesystem::path(root_out).append(fileName + "_grade_full.geojson").string();
//         std::string out_mid_points = std::filesystem::path(root_out).append(fileName + "_mid_points.geojson").string();
//         std::string out_path = std::filesystem::path(root_out).append(fileName + "_path.geojson").string();
//
//         double central = 0.0;
//         std::vector<studio_geo_coll> src_colls;
//         studio_poly gs_src_poly;
//         if (!studio_geo_utils::read_geo_coll(fence, src_colls))
//         {
//             std::cout << "read geojson failed" << std::endl;
//             continue;
//         }
//         double left = 1000, right = -1000, top = -1000, bottom = 1000;
//         double gs_left = 100000000, gs_right = -1000, gs_top = -1000, gs_bottom = 100000000;
//         for (auto& coll : src_colls)
//         {
//             if (coll.m_type == enum_geometry_type::egtPolygon)
//             {
//                 if (coll.m_poly.outer_ring.points.empty())
//                 {
//                     break;
//                 }
//                 central = floor((coll.m_poly.outer_ring.points.begin()->x + 1.5) / 3.0) * 3.0;
//                 for (const auto& point : coll.m_poly.outer_ring.points)
//                 {
//                     double t_lon = point.x;
//                     double t_lat = point.y;
//                     left = std::min(left, t_lon);
//                     right = std::max(right, t_lon);
//                     top = std::max(top, t_lat);
//                     bottom = std::min(bottom, t_lat);
//                     // 经纬度转高斯
//                     double gx = 0.0, gy = 0.0;
//                     proj::lonlat_to_gauss(central, point.x, point.y, gx, gy);
//                     gs_src_poly.outer_ring.points.push_back(studio_point(gx, gy));
//                     gs_left = std::min(gs_left, gx);
//                     gs_right = std::max(gs_right, gx);
//                     gs_top = std::max(gs_top, gy);
//                     gs_bottom = std::min(gs_bottom, gy);
//                 }
//             }
//         }
//
//         studio_rect rect(left, top, right, bottom);
//         x_scan_line xlr_gcs;
//         // scan_line_algo xlr_gcs;
//         double cell_size = 0.0001;
//         xlr_gcs.set(rect, cell_size);
//         xlr_gcs.rasterize(src_colls.begin()->m_poly);
//         xlr_gcs.fill();
//         xlr_gcs.blocks();
//         // xlr_gcs.scanning(gs_src_poly);
//         // 生成中线点并存储到 studio_multi_point 对象
//
//         // 扫描线栅格中心点保存文件
//         studio_multi_point grid_mid_points;
//         xlr_gcs.get_grid_mid_points(grid_mid_points);
//         studio_geo_utils::write_geo(out_mid_points, grid_mid_points, enum_geometry_type::egtMultiPoint);
//
//         // 扫描线栅格保存文件
//         std::vector<studio_poly> temp_polys = xlr_gcs.grids();
//         studio_multi_poly temp_grad;
//         for (auto& poly : temp_polys)
//         {
//             temp_grad.push_back(poly);
//         }
//         studio_geo_utils::write_geo(out_grade_full, temp_grad, enum_geometry_type::egtMultiPolygon);
//
//         std::map<int, std::vector<int>> all_blocks = xlr_gcs.get_blocks();
//
//         // studio_wave_cover_ccpp planner(xlr_gcs.width(), xlr_gcs.height());
//         // // 设置电子围栏区域
//         // planner.setFencedArea(all_blocks);
//         // // 正确获取起始点坐标
//         // int start_y = all_blocks.begin()->first;        // 行号 (y坐标)
//         // int start_x = all_blocks.begin()->second.at(0); // 列号 (x坐标)
//         // planner.setStartPosition({start_x, start_y});  // (x, y)
//         // // 执行路径规划
//         // planner.planPath();
//         // std::vector<Position> grad_path = planner.getOptimalPath();
//
//         // 创建路径规划器
//         studio_plow_cover_ccpp planner(all_blocks);
//
//         // 生成路径
//         std::vector<std::pair<int, int>> grad_path = planner.generate_path();
//
//         studio_line gcs_full_path;
//         xlr_gcs.get_grad_mid_path(grad_path, gcs_full_path);
//         studio_geo_utils::write_geo(out_path, gcs_full_path, enum_geometry_type::egtLineString);
//
//         // 一个文件处理结束
//         // break;
//     }
//
//     geo_utils::destroy_gdal_env();
//     return 0;
// }


// 波前路径全覆盖算法

// int main()
// {
//     std::cout << "\r\n\r\n****************" << "full_paht_coverage" << "****************" << std::endl;
//     std::string root_out;
//     std::string fence_root;
// #ifdef IS_WINDOWS
//         root_out = "D:/9_data/2_readfile/geojson/full_cover/";
//         fence_root = "D:/9_data/2_readfile/geojson/fence/";
// #else
//     root_out = "/home/wyj/myself/2_data/2_geojson/full_cover/";
//     fence_root = "/home/wyj/myself/2_data/2_geojson/fence/";
// #endif
//
//     studio_geo_utils::init_gdal_env();
//
//     std::vector<std::string> all_fences = studio_dir::getFilesOrDirs(fence_root);
//     for (auto& fence : all_fences)
//     {
//         std::string fileName = std::filesystem::path(fence).filename().stem().string();
//         std::string out_grade = std::filesystem::path(root_out).append(fileName + "_grade.geojson").string();
//         std::string out_grade_full = std::filesystem::path(root_out).append(fileName + "_grade_full.geojson").string();
//         std::string out_mid_points = std::filesystem::path(root_out).append(fileName + "_mid_points.geojson").string();
//         std::string out_path = std::filesystem::path(root_out).append(fileName + "_path.geojson").string();
//
//         double central = 0.0;
//         std::vector<studio_geo_coll> src_colls;
//         studio_poly gs_src_poly;
//         if (!studio_geo_utils::read_geo_coll(fence, src_colls))
//         {
//             std::cout << "read geojson failed" << std::endl;
//             continue;
//         }
//         double left = 1000, right = -1000, top = -1000, bottom = 1000;
//         double gs_left = 100000000, gs_right = -1000, gs_top = -1000, gs_bottom = 100000000;
//         for (auto& coll : src_colls)
//         {
//             if (coll.m_type == enum_geometry_type::egtPolygon)
//             {
//                 if (coll.m_poly.outer_ring.points.empty())
//                 {
//                     break;
//                 }
//                 central = floor((coll.m_poly.outer_ring.points.begin()->x + 1.5) / 3.0) * 3.0;
//                 for (const auto& point : coll.m_poly.outer_ring.points)
//                 {
//                     double t_lon = point.x;
//                     double t_lat = point.y;
//                     left = std::min(left, t_lon);
//                     right = std::max(right, t_lon);
//                     top = std::max(top, t_lat);
//                     bottom = std::min(bottom, t_lat);
//                     // 经纬度转高斯
//                     double gx = 0.0, gy = 0.0;
//                     proj::lonlat_to_gauss(central, point.x, point.y, gx, gy);
//                     gs_src_poly.outer_ring.points.push_back(studio_point(gx, gy));
//                     gs_left = std::min(gs_left, gx);
//                     gs_right = std::max(gs_right, gx);
//                     gs_top = std::max(gs_top, gy);
//                     gs_bottom = std::min(gs_bottom, gy);
//                 }
//             }
//         }
//
//         studio_rect rect(left, top, right, bottom);
//         x_scan_line xlr_gcs;
//         // scan_line_algo xlr_gcs;
//         double cell_size = 0.0001;
//         xlr_gcs.set(rect, cell_size);
//         xlr_gcs.rasterize(src_colls.begin()->m_poly);
//         xlr_gcs.fill();
//         xlr_gcs.blocks();
//         // xlr_gcs.scanning(gs_src_poly);
//         // 生成中线点并存储到 studio_multi_point 对象
//
//         // 扫描线栅格中心点保存文件
//         studio_multi_point grid_mid_points;
//         xlr_gcs.get_grid_mid_points(grid_mid_points);
//         studio_geo_utils::write_geo(out_mid_points, grid_mid_points, enum_geometry_type::egtMultiPoint);
//
//         // 扫描线栅格保存文件
//         std::vector<studio_poly> temp_polys = xlr_gcs.grids();
//         studio_multi_poly temp_grad;
//         for (auto& poly : temp_polys)
//         {
//             temp_grad.push_back(poly);
//         }
//         studio_geo_utils::write_geo(out_grade_full, temp_grad, enum_geometry_type::egtMultiPolygon);
//
//         std::map<int, std::vector<int>> all_block = xlr_gcs.get_blocks();
//
//         studio_wave_cover_ccpp planner(xlr_gcs.width(), xlr_gcs.height());
//         // 设置电子围栏区域
//         planner.setFencedArea(all_block);
//         // 正确获取起始点坐标
//         int start_y = all_block.begin()->first;        // 行号 (y坐标)
//         int start_x = all_block.begin()->second.at(0); // 列号 (x坐标)
//         planner.setStartPosition({start_x, start_y});  // (x, y)
//         // 执行路径规划
//         planner.planPath();
//         std::vector<Position> grad_path = planner.getOptimalPath();
//
//         studio_line gcs_full_path;
//         xlr_gcs.get_grad_mid_path(grad_path, gcs_full_path);
//         studio_geo_utils::write_geo(out_path, gcs_full_path, enum_geometry_type::egtLineString);
//
//         // 一个文件处理结束
//         // break;
//     }
//
//     geo_utils::destroy_gdal_env();
//     return 0;
//
//     std::string fence_1_path = std::filesystem::path(root_out).append("fence_1.geojson").string();
//     double central = 0.0;
//     std::vector<studio_geo_coll> collections;
//     studio_poly gs_src_poly;
//     bool re = studio_geo_utils::read_geo_coll(fence_1_path, collections);
//     double left = 1000, right = -1000, top = -1000, bottom = 1000;
//     double gs_left = 100000000, gs_right = -1000, gs_top = -1000, gs_bottom = 100000000;
//     for (auto& coll : collections)
//     {
//         if (coll.m_type == enum_geometry_type::egtPolygon)
//         {
//             if (coll.m_poly.outer_ring.points.empty())
//             {
//                 break;
//             }
//             central = floor((coll.m_poly.outer_ring.points.begin()->x + 1.5) / 3.0) * 3.0;
//             for (const auto& point : coll.m_poly.outer_ring.points)
//             {
//                 double t_lon = point.x;
//                 double t_lat = point.y;
//                 left = std::min(left, t_lon);
//                 right = std::max(right, t_lon);
//                 top = std::max(top, t_lat);
//                 bottom = std::min(bottom, t_lat);
//                 // 经纬度转高斯
//                 double gx = 0.0, gy = 0.0;
//                 proj::lonlat_to_gauss(central, point.x, point.y, gx, gy);
//                 gs_src_poly.outer_ring.points.push_back(studio_point(gx, gy));
//
//                 gs_left = std::min(gs_left, gx);
//                 gs_right = std::max(gs_right, gx);
//                 gs_top = std::max(gs_top, gy);
//                 gs_bottom = std::min(gs_bottom, gy);
//             }
//         }
//         break;
//     }
//     // 经纬度计算
//     studio_rect rect(left, top, right, bottom);
//     x_scan_line xlr;
//     // scan_line_algo xlr;
//     double cell_size = 0.0001;
//     xlr.set(rect, cell_size);
//     xlr.rasterize(collections.begin()->m_poly);
//     xlr.fill();
//     xlr.blocks();
//     // xlr.scanning(collections.begin()->m_poly);
//     // 生成中线点并存储到 studio_multi_point 对象
//     studio_multi_point grid_mid_points;
//     xlr.get_grid_mid_points(grid_mid_points);
//     std::vector<studio_geo_coll> out_collections;
//     studio_geo_coll temp_geo_coll;
//     temp_geo_coll.m_type = enum_geometry_type::egtMultiPoint;
//     temp_geo_coll.m_points = grid_mid_points;
//     out_collections.push_back(temp_geo_coll);
//     std::string out_path = std::filesystem::path(root_out).append("scan_gcs_mult_point_1.geojson").string();
//     // studio_geo_utils::write_geo_coll(out_path, out_collections);
//
//     std::vector<studio_geo_coll> gcs_grad_out;
//     studio_geo_coll temp_gcs_grad;
//     temp_gcs_grad.m_type = enum_geometry_type::egtMultiPolygon;
//     std::vector<studio_poly> temp_gcs_polys = xlr.grids();
//     for (auto& poly : temp_gcs_polys)
//     {
//         temp_gcs_grad.m_polys.push_back(poly);
//     }
//     gcs_grad_out.push_back(temp_gcs_grad);
//     std::string grad_gcs_out_path = std::filesystem::path(root_out).append("scan_gcs_grad_1.geojson").string();
//     // studio_geo_utils::write_geo_coll(grad_gcs_out_path, gcs_grad_out);
//     std::map<int, std::vector<int>> all_block = xlr.get_blocks();
//
//     studio_wave_cover_ccpp planner(xlr.width(), xlr.height());
//     // 设置电子围栏区域
//     planner.setFencedArea(all_block);
//     int beg_x = all_block.begin()->first;
//     int beg_y = all_block.begin()->second.at(0);
//     planner.setStartPosition({2, 3});
//     // 执行路径规划
//     planner.planPath();
//     std::vector<Position> grad_path = planner.getOptimalPath();
//
//     studio_line gcs_full_path;
//     xlr.get_grad_mid_path(grad_path, gcs_full_path);
//     std::vector<studio_geo_coll> gcs_full_path_out;
//     studio_geo_coll temp_gcs_full_path;
//     temp_gcs_full_path.m_type = enum_geometry_type::egtLineString;
//     temp_gcs_full_path.m_line = gcs_full_path;
//     gcs_full_path_out.push_back(temp_gcs_full_path);
//     std::string gcs_full_path_out_path = std::filesystem::path(root_out).append("scan_gcs_full_path_1.geojson").string();
//     studio_geo_utils::write_geo_coll(gcs_full_path_out_path, gcs_full_path_out);
//
//     // generateGridAndSave("/home/wyj/myself/2_data/2_geojson/scan//grid_2.geojson", top, bottom, left, right, cell_size);
//     studio_geo_utils::destroy_gdal_env();
//     return 0;
//
//     // 高斯计算
//     studio_rect gs_rect(gs_left, gs_top, gs_right, gs_bottom);
//     x_scan_line xlr_gs;
//     // scan_line_algo xlr_gs;
//     double gs_cell_size = 10;
//     xlr_gs.set(gs_rect, gs_cell_size);
//     xlr_gs.rasterize(gs_src_poly);
//     xlr_gs.fill();
//     // xlr_gs.image(std::filesystem::path(root_out).append("gs_xlr_1.png").string());
//     // xlr_gs.scanning(gs_src_poly);
//     // 生成中线点并存储到 studio_multi_point 对象
//     studio_multi_point gs_grid_mid_points;
//     xlr_gs.get_grid_mid_points(gs_grid_mid_points);
//     // // 高斯转经纬度
//     // studio_multi_point gcs_grid_mid_points;
//     // for (auto& point : gs_grid_mid_points)
//     // {
//     //     double lon = 0.0, lat = 0.0;
//     //     proj::gauss_to_lonlat(central, point.x, point.y, lon, lat);
//     //     gcs_grid_mid_points.push_back(studio_point(lon, lat));
//     // }
//     std::vector<studio_geo_coll> gs_out_collections;
//     studio_geo_coll gs_temp_geo_coll;
//     gs_temp_geo_coll.m_type = enum_geometry_type::egtMultiPoint;
//     // gs_temp_geo_coll.m_points = gcs_grid_mid_points;
//     gs_temp_geo_coll.m_points = gs_grid_mid_points;
//     std::vector<studio_geo_coll> temp_gs_geo_coll;
//     temp_gs_geo_coll.push_back(gs_temp_geo_coll);
//     std::string gs_out_path = std::filesystem::path(root_out).append("scan_gs_grad_1.geojson").string();
//     studio_geo_utils::write_geo_coll(gs_out_path, temp_gs_geo_coll);
//
//     // generateGridAndSave("/home/wyj/myself/2_data/2_geojson/scan//grid_2.geojson", top, bottom, left, right, cell_size);
//
//     std::vector<studio_geo_coll> grad_out;
//     studio_geo_coll temp_grad;
//     temp_grad.m_type = enum_geometry_type::egtMultiPolygon;
//     std::vector<studio_poly> temp_polys = xlr_gs.grids();
//     for (auto& poly : temp_polys)
//     {
//         temp_grad.m_polys.push_back(poly);
//     }
//     grad_out.push_back(temp_grad);
//     std::string grad_out_path = std::filesystem::path(root_out).append("scan_gs_mulit_point_1.geojson").string();
//     studio_geo_utils::write_geo_coll(grad_out_path, grad_out);
//
//     geo_utils::destroy_gdal_env();
//
//     return 0;
//
//     int e = 0;
//     int f = 9;
//     int g = 8;
// }